#pragma once
#include "kinematic_base.h"
#include <iostream>
#include <float.h>

namespace actuator
{
class AMRKinematicModeNormal: public AMRKinematicModeBase
{
public:
    AMRKinematicModeNormal();
    ~AMRKinematicModeNormal();

    bool Init(std::string &path);
    void KinematicMode(float speed, float angle_speed, float speed_towards, eMoveMode mode, AMRCmdAndState &amr_state);
    void VehicleInverseMode(AMRCmdAndState &amr_state, float &speed, float &angle_speed, float &steer_angle);

private:
    void CalcSteerCmd(const float speed, const float angle_speed, const float speed_towards, float &x, float &y, SteerCmd &steer_cmd, eMoveMode mode);
    void restain_steer_angle(float & angle, float & speed);
    
    float min_radius_;
    float max_speed_;
    uint8_t control_stage_;
    
    eMoveMode last_mode_;

    float max_steer_angle_;
    float max_steer_speed_;

    float zero_speed_;

    common_lib::Common com_;
};
}